#include "fsm_msg.h"
#include "Bsp.h"

void fsm_msg_init(fsm_msg_t *msg_ctr, uint8_t *msg_pool, uint32_t msg_cnt, uint32_t msg_size)
{
    msg_ctr->msg_rx_cnt     = 0;
    msg_ctr->msg_cnt        = msg_cnt;
    msg_ctr->msg_size       = msg_size;
    msg_ctr->msg_pool       = msg_pool;
    msg_ctr->msg_read_idx   = msg_pool;
    msg_ctr->msg_write_idx  = msg_pool;
}

int fsm_msg_write(fsm_msg_t *msg_ctr, uint8_t *pData, uint32_t DatLen)
{
    int ret = 0;

    if (DatLen > msg_ctr->msg_size)
    {
        DatLen = msg_ctr->msg_size;
    }
    
    if(msg_ctr->msg_rx_cnt < msg_ctr->msg_cnt)
    {
        Bsp_hw_interrupt_disable();

        memcpy(msg_ctr->msg_write_idx, pData, DatLen);
        
        msg_ctr->msg_rx_cnt ++;
        msg_ctr->msg_write_idx += msg_ctr->msg_size;
        if(msg_ctr->msg_write_idx >= &msg_ctr->msg_pool[msg_ctr->msg_cnt * msg_ctr->msg_size])
        {
            msg_ctr->msg_write_idx = msg_ctr->msg_pool;
        }

        Bsp_hw_interrupt_enable();

        ret = 1;
    }

    return ret;
}

int fsm_msg_read(fsm_msg_t *msg_ctr, uint8_t *pData)
{
    int ret = 0;

    if(msg_ctr->msg_rx_cnt > 0)
    {
        Bsp_hw_interrupt_disable();

        memcpy(pData, msg_ctr->msg_read_idx, msg_ctr->msg_size);
        
        msg_ctr->msg_rx_cnt --;
        msg_ctr->msg_read_idx += msg_ctr->msg_size;
        if(msg_ctr->msg_read_idx >= &msg_ctr->msg_pool[msg_ctr->msg_cnt * msg_ctr->msg_size])
        {
            msg_ctr->msg_read_idx = msg_ctr->msg_pool;
        }

        Bsp_hw_interrupt_enable();

        ret = 1;
    }

    return ret;
}
